#include "vehicle_controller.h"
#include "communication/typetransfer.h"
#include "communication/utility.hpp"
#include "main_controller.h"
VehicleController::VehicleController(MainController *main_controller, const double v_limit, const double w_limit):
    BaseController (main_controller,kVehicleIndex),v_limit_(fabs(v_limit)),w_limit_(fabs(w_limit))
{

}

ErrorCode VehicleController::Update()
{
    return kOkay;
}

bool VehicleController::ParseUsrData(const u_char *buffer, const size_t buffer_len)
{
    if(buffer_len!=(1+1+sizeof (SocketDataType)*2+2))//part+attr+data(v & w)+crc
        return false;

    size_t index=0;
//    u_char part=0;
//    TypeTransfer<u_char>::ByteArray2Type(part,buffer+index);
    index+=sizeof (u_char);
    u_char attr=0;
    TypeTransfer<u_char>::ByteArray2Type(attr,buffer+index);
    index+=sizeof (attr);



    if(attr&Slave2Master::VehicleAttrType::kVehicleVel)
    {
        SocketDataType data;
        TypeTransfer<SocketDataType>::ByteArray2Type(data,buffer+index);
        cur_linear_vel_=static_cast<double>(data);
        index+=sizeof (data);

        TypeTransfer<SocketDataType>::ByteArray2Type(data,buffer+index);
        cur_angular_vel_=static_cast<double>(data);
        index+=sizeof (data);
        return true;
    }
    return false;
}



bool VehicleController::PackUsrData(u_char *buffer, size_t &buffer_len)
{
    if(updated_attr_==0x00)
    {
        buffer_len=0;
        return false;
    }
    size_t index=0;
    u_char type=GetSlaveTargetIndex();
    TypeTransfer<u_char>::Type2ByteArray(buffer+index,type);
    index+=sizeof (type);
    //u_char updated_attr_=Master2Slave::VehicleAttrType::kVehicleVel;
    TypeTransfer<u_char>::Type2ByteArray(buffer+index,updated_attr_);
    index+=sizeof (updated_attr_);

    if(updated_attr_&Master2Slave::VehicleAttrType::kVehicleVel)
    {
        TypeTransfer<SocketDataType>::Type2ByteArray(buffer+index,static_cast<SocketDataType>(target_linear_vel_));
        index+=sizeof (SocketDataType);
        TypeTransfer<SocketDataType>::Type2ByteArray(buffer+index,static_cast<SocketDataType>(target_angular_vel_));
        index+=sizeof (SocketDataType);
    }

    ClearTargetStatus();
    if(index<=2)
    {
        buffer_len=0;
        return false;
    }
    else
    {
        buffer_len=index;
        return true;
    }
}

void VehicleController::SendTarget2Slave(const double v, const double w)
{
    if(*master_device_index_<0)
    {
        ClearTargetStatus();
        return;
    }
    cur_linear_vel_=fmax(fmin(v,v_limit_),-v_limit_);
    cur_angular_vel_=fmax(fmin(w,w_limit_),-w_limit_);
    updated_attr_=updated_attr_|Master2Slave::VehicleAttrType::kVehicleVel;

}

void VehicleController::GenVehicleVel(const double px,const double py) const
{

}
